#include "CollisionManager.h"

#include "../Math/Vector3.h"

bool CollisionManager::SphereToAABB( Sphere* sphere, AABB* aabb )
{
	Vector3 centerPoint = sphere->center;

	for(unsigned i = 0; i < 3; ++i)
	{
		if	   ( centerPoint.arr[i] < aabb->minPoint.arr[i] )
		{
			centerPoint.arr[i] = aabb->minPoint.arr[i];
		}
		else if( centerPoint.arr[i] > aabb->maxPoint.arr[i] )
		{
			centerPoint.arr[i] = aabb->maxPoint.arr[i];
		}
	}

	Vector3 vec = sphere->center - centerPoint;

	if( DotProduct( vec, vec ) < (sphere->radius * sphere->radius) )
	{
		return true;
	}

	return false;
}

bool CollisionManager::SphereToSphere( Sphere* sphereA, Sphere* sphereB )
{
	Vector3 vecTo = sphereA->center - sphereB->center;
	float distance = DotProduct( vecTo, vecTo );
	float totalRadius = sphereA->radius + sphereB->radius;

	if( distance < (totalRadius * totalRadius) )
	{
		return true;
	}

	return false;
}

bool CollisionManager::SphereToTriangle( Sphere* sphere, Triangle* triangle )
{
	const Vector3 triangleNormal = triangle->GetNormal();

	float offset = DotProduct( triangleNormal, sphere->center - triangle->GetVertex(0) );

	if( offset > sphere->radius || offset < -FLT_EPSILON )
		return false;

	Vector3 triCenter = sphere->center - triangleNormal * offset;

	for(unsigned int i = 0; i < 3; ++i)
	{
		Vector3 edge = triangle->GetVertex( (1<<i) & (0x3) ) - triangle->GetVertex(i);
		Vector3 norm = CrossProduct(edge, triangleNormal);

		Vector3 vec = triCenter - triangle->GetVertex(i);
		if( DotProduct( norm, vec ) < 0.0f )
			continue;

		float ratio = DotProduct( vec, edge ) / DotProduct( edge, edge );

		if( ratio < 0.0f )
			ratio = 0.0f;
		if( ratio > 1.0f )
			ratio = 1.0f;

		triCenter = triangle->GetVertex(i) + edge * ratio;
		break;
	}

	Vector3 vecTo = sphere->center - triCenter;

	if( DotProduct( vecTo, vecTo ) < (sphere->radius * sphere->radius) )
	{
		return true;
	}

	return false;
}